/*
 * Copyright (C) 2013 by KB3ZYT Richard Nash
 * 
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
 */

#include "GPS.h"

// Constructor using SoftwareSerial
GPS::GPS(SoftwareSerial *ser)
{
    hour = minute = seconds = year = month = day =
    fixquality = satellites = 0; // uint8_t
    fix = false; // boolean
    latitude = longitude = altitude = speed = angle = 0.0; // float
    gpsSwSerial = ser;
#ifdef SIMULATE
    millisSinceLastSend = millis();
    simCount = 0;
#endif
}

void GPS::init(uint16_t baud)
{
    gpsSwSerial->begin(baud);
    delay(10);
    gpsSwSerial->listen();
    delay(10);
}

float GPS::degreesAndFractionalMinutesToDegrees(float deg, char hemi)
{
    float degPart = (int)(deg/100.0);
    deg = (deg - degPart*100.0) / 60.0 + degPart;
    if (hemi == 'S' || hemi == 'W') {
        return -deg;
    } else {
        return deg;
    }
}

uint8_t GPS::parseGGA(const char* nmea)
{
    // These are calculated but thrown away
    uint16_t milliseconds = 0;
    float geoidheight;
    float HDOP;
    char lat, lon;
    
    // Assuming a GGA sentence
    const char* p = nmea;
    // get time
    p = strchr(p, ',')+1;
    float timef = atof(p);
    uint32_t time = timef;
    hour = time / 10000;
    minute = (time % 10000) / 100;
    seconds = (time % 100);
    
    milliseconds = fmod(timef, 1.0) * 1000;
    
    // parse out latitude
    p = strchr(p, ',')+1;
    latitude = atof(p);
    
    p = strchr(p, ',')+1;
    if (p[0] == 'N') lat = 'N';
    else if (p[0] == 'S') lat = 'S';
    else if (p[0] == ',') lat = 0;
    else { 
        return PARSE_ERROR;
    }
    latitude = degreesAndFractionalMinutesToDegrees(latitude, lat);
    
    // parse out longitude
    p = strchr(p, ',')+1;
    longitude = atof(p);
    
    p = strchr(p, ',')+1;
    if (p[0] == 'W') lon = 'W';
    else if (p[0] == 'E') lon = 'E';
    else if (p[0] == ',') lon = 0;
    else { 
        return PARSE_ERROR;
    }
    longitude = degreesAndFractionalMinutesToDegrees(longitude, lon);
    
    p = strchr(p, ',')+1;
    fixquality = atoi(p);
    
    p = strchr(p, ',')+1;
    satellites = atoi(p);
    
    p = strchr(p, ',')+1;
    HDOP = atof(p);
    
    p = strchr(p, ',')+1;
    altitude = atof(p);
    p = strchr(p, ',')+1;
    p = strchr(p, ',')+1;
    geoidheight = atof(p);
    return PARSED_SOMETHING;    
}

uint8_t GPS::parseRMC(const char* nmea)
{
    // These are calculated but thrown away
    uint16_t milliseconds = 0;
    char lat, lon;
    
    // found RMC
    const char* p = nmea;
    
    // get time
    p = strchr(p, ',')+1;
    float timef = atof(p);
    uint32_t time = timef;
    hour = time / 10000;
    minute = (time % 10000) / 100;
    seconds = (time % 100);
    
    milliseconds = fmod(timef, 1.0) * 1000;
    
    p = strchr(p, ',')+1;
    if (p[0] == 'A') 
        fix = true;
    else if (p[0] == 'V')
        fix = false;
    else
        return PARSE_ERROR;
    
    // parse out latitude
    p = strchr(p, ',')+1;
    latitude = atof(p);
    
    p = strchr(p, ',')+1;
    if (p[0] == 'N') lat = 'N';
    else if (p[0] == 'S') lat = 'S';
    else if (p[0] == ',') lat = 0;
    else return false;
    latitude = degreesAndFractionalMinutesToDegrees(latitude, lat);
    
    // parse out longitude
    p = strchr(p, ',')+1;
    longitude = atof(p);
    
    p = strchr(p, ',')+1;
    if (p[0] == 'W') lon = 'W';
    else if (p[0] == 'E') lon = 'E';
    else if (p[0] == ',') lon = 0;
    else return PARSE_ERROR;
    longitude = degreesAndFractionalMinutesToDegrees(longitude, lon);
    
    // speed
    p = strchr(p, ',')+1;
    speed = atof(p);
    
    // angle
    p = strchr(p, ',')+1;
    angle = atof(p);
    
    p = strchr(p, ',')+1;
    uint32_t fulldate = atof(p);
    day = fulldate / 10000;
    month = (fulldate % 10000) / 100;
    year = (fulldate % 100);
    
    return PARSED_SOMETHING;
}

// checks to see if a sentence is available. If so
// it reads it into the buffer provided by the caller.
uint8_t GPS::recieve(char* nmea, int len) {
    if(gpsSwSerial->available() == 0) 
        return 0;
    
    // Read the sentence up to a newline
    len--; // Leave room for trailing 0
    int i = 0;
    char c = 0;
    uint8_t done = 0;
    while (!done) {
        while (!done && gpsSwSerial->available() > 0) {
            c = gpsSwSerial->read();
            nmea[i++] = c;
            if (c == '\n' || i == len) done = 1;
        }
        while(!done && gpsSwSerial->available() == 0) {
            // Spin until more characters becomes available   
        } 
    }
    nmea[i] = 0;
    
    return 1;
}

uint8_t GPS::parse(const char* nmea)
{
        Serial.print(F(">"));
        Serial.print(nmea);
        Serial.println(F("<"));
    if (strlen(nmea) < 65) {
        // This is too short ot be a sentence I can parse
        return PARSE_NONE_RECEIVED;
    }
    
    {
        uint8_t parsed = PARSE_ERROR;
        GPS savedGPS(gpsSwSerial);
        // Save everything.
        memcpy(&savedGPS, this, sizeof(GPS));
        
        // look for a few common sentences at the beginning of the sentence
        if (strstr(nmea, "$GPGGA") == nmea) {
            parsed = parseGGA(nmea);
        } else if (strstr(nmea, "$GPRMC") == nmea) {
            parsed = parseRMC(nmea);
        }
        if (parsed != PARSED_SOMETHING) {
            // If parsing fails, restore the last save location information
            memcpy(&savedGPS, this, sizeof(GPS));
        }
        // Not a sentence I recognize, ignore it.
        return parsed;
    }
}

// This needs to be called frequently enough so that
// the entire sentence doesn't get sent otherwise bits of it will be lost
// because the input buffer isn't large enough to hold it.
uint8_t GPS::recieveGPSSentence()
{
#ifdef SIMULATE
    if (millis()< millisSinceLastSend + 4000) return PARSE_NONE_RECEIVED;
    hour = (simCount*4) / 60;
    minute = (simCount*4) % 60;
    seconds = 0;
    year = 13;
    month = 4;
    day = 0;
    if (simCount < 10) {
      // Awaiting launch
      longitude = -79.1234;
      altitude = 300;
    } else if (simCount >= 10 && simCount < 30) {
        // Rising
        longitude = -79.1234 + (simCount-10);
        altitude = 300 + (simCount-10) * 100;
    } else if (simCount >= 30 && simCount < 50) {
        // Falling
        longitude = -79.1234 + (simCount-10);
        altitude = 300 + (20) * 100 - (simCount-30)*100;
    } else if (simCount >= 50) {
        // On ground
        longitude = -79.1234 + 40;
        altitude = 300;
    }
    latitude = 41.2345;
    speed = 0;
    angle = 0;
    fix = 1;
    fixquality = 2;
    satellites = 4;
    millisSinceLastSend = millis();
    simCount++;
    return PARSED_SOMETHING;
#else
    char nmea[100];
    if (!recieve(nmea,100)) return PARSE_NONE_RECEIVED;
    uint8_t parsed = PARSE_NONE_RECEIVED;
    uint8_t done = 1;
    do {
        uint8_t parse2 = parse(nmea);
        if (parse2 > parsed) parsed = parse2;
        done = 1;
        unsigned long timeout = millis() + 20; // Wait 2/100ths of a second for more data
        while (millis() < timeout) {
            if (recieve(nmea,100)) {
                timeout = millis() + 20; // Reset timer
                done = 0;
                break;
            }
        }
    } while (!done);
    return parsed;
#endif
}



void GPS::sendCommand(const char* str) {
#ifdef SIMULATE
// Do nothing if in simulation mode
#else
    gpsSwSerial->println(str);
#endif
}


